Lab. 02 - ROS2 Actions

1. Czym są Akcje w ROS2?

Akcje są jednym z typów komunikacji w ROS2, przeznaczonym do zadań, które trwają dłużej. Składają się z trzech części: celu (goal), informacji zwrotnej (feedback) i wyniku (result).

Akcje zbudowane są na topicach i serwisach. Ich sposób działania przypomina serwisy, z dwoma różnicami:

• mogą być zatrzymane przed zakończeniem działania,
• dostarczają ciągłą informację zwrotną w czasie pracy, a serwis tylko jednorazowo informuje o efekcie na koniec wykonywania.

Cel (goal) - służy wywołaniu akcji. Ma funkcjonalność serwisu- wywołanie i odpowiedź, czy udało się uruchomić akcję.

Informacja zwrotna (feedback) - są to dane publikowane przez serwer akcji w czasie pracy. Zazwyczaj informują o aktualnym postępie działania, np. jaka odległość jeszcze została do przejechania, jaki jest szacowany pozostały czas albo w ilu procentach działanie zostało zakończone.

Wynik (result) - ostateczna odpowiedź serwera akcji na temat wyniku działania. Zazwyczaj zawiera binarną odpowiedź czy udało się wykonać żądane działanie lub z jaką dokładnością akcja została przeprowadzona.

2. Przygotowanie środowiska

Przed przystąpieniem do pracy należy przygotować środowisko: Instrukcja przygotowania środowiska do zajęć. Obraz bazuje na osrf/ros:lyrical-desktop-full (ROS 2 Lyrical Luth).

Domyślnie kontener nosi nazwę ARM_02.

UWAGA! Skrypty po uruchomieniu usuwają kontener o takiej nazwie przed utworzeniem nowego.

Korzystanie z kontenera

Po każdym ponownym uruchomieniu komputera, proszę pamiętać o wywoływaniu:

xhost +local:root

Nowy terminal można dołączyć do kontenera korzystając z polecenia:

docker exec -it ARM_02 bash

ROS_DOMAIN_ID

W przypadku pracy na wielu komputerach w tej samej sieci lokalnej (eduroam też może tego wymagać), konieczne może być ustawienie zmiennej środowiskowej ROS_DOMAIN_ID na różnych wartościach. Należy wybrać dowolną liczbę całkowitą z zakresu 0-100. Wartość tę należy ustawić w pliku ~/.bashrc:

export ROS_DOMAIN_ID=10  # przykładowa wartość

lub bezpośrednio w terminalu (wtedy będzie obowiązywać tylko w tym terminalu):

export ROS_DOMAIN_ID=10  # przykładowa wartość

Sugerowane jest użycie dwóch ostatnich cyfr numeru IP komputera jako wartości ROS_DOMAIN_ID, aby uniknąć konfliktów z innymi użytkownikami w sieci. Aby sprawdzić swój adres IP, można użyć polecenia:

ip addr show

adres IP będzie widoczny przy interfejsie sieciowym, np. wlan0 lub eth0.

Wygodne może być dodanie tego exportu do bashrc:

echo 'export ROS_DOMAIN_ID=<wybrana_wartość>' >> ~/.bashrc 

Gazebo: GZ_IP i GZ_PARTITION

Gazebo komunikuje się niezależnie od ROS 2 (biblioteka gz-transport), więc ROS_DOMAIN_ID nie rozdziela symulacji. Domyślnie Gazebo łączy ze sobą symulacje uruchomione przez tego samego użytkownika na komputerach o tej samej nazwie (hostname), a kontener uruchomiony z --network=host przejmuje nazwę komputera i działa jako root. Jeżeli komputery w sieci mają takie same nazwy, symulacje różnych osób mogą się wzajemnie zakłócać.

Dlatego w obrazie, w pliku ~/.bashrc, ustawiona jest zmienna GZ_IP=127.0.0.1, dzięki której Gazebo komunikuje się tylko w obrębie jednego komputera. W kontenerach utworzonych ze starszej wersji obrazu (polecenie echo $GZ_IP nic nie wypisuje) należy ją dopisać samodzielnie:

echo 'export GZ_IP=127.0.0.1' >> ~/.bashrc

Jeżeli na jednym komputerze działa kilka symulacji (np. w kilku kontenerach), można je dodatkowo rozdzielić, ustawiając w każdym kontenerze inną wartość zmiennej GZ_PARTITION (węzły Gazebo widzą się nawzajem tylko w obrębie tej samej partycji):

echo 'export GZ_PARTITION=<wybrana_nazwa>' >> ~/.bashrc  # np. arm_10

Wartość GZ_PARTITION musi być taka sama we wszystkich terminalach, w których korzysta się z Gazebo (uruchomienie symulacji, polecenia gz), dlatego lepiej ustawić ją w ~/.bashrc niż w pojedynczym terminalu. Zmiany w ~/.bashrc obowiązują w nowo otwartych terminalach, więc po ich wprowadzeniu należy ponownie uruchomić symulację.

3. Cel zajęć

Docelowo na zajęciach zadaniem będzie utworzenie akcji która porusza robota na zadaną odległość od przeszkody, która się przed nim znajduje, przekazując również czas na wykonanie zadania i minimalna dokładność, jaka jest wymagana. W feedbacku będzie publikowana informacja o aktualnej odległości od przeszkody i przewidywanym czasie pozostałym do zakończenia wykonywania akcji. Jako result serwer będzie informował czy udało się spełnić założenia, jaka jest ostateczna dokładność i odległość od celu oraz ile czasu trwała procedura.

4. Tworzenie akcji

Zanim rozpoczniemy prace nad powyższym zadaniem, warto zapoznać się z definiowaniem akcji na prostszym przykładzie - obliczaniu ciągu Fibonacciego.

Tworzenie nowej paczki

Aby utworzyć nową paczkę, należy, z poziomu /arm_ws/src wywołać polecenie:

ros2 pkg create --build-type ament_cmake fibonacci_sequence_action

Konieczne będzie też utworzenie katalogu action i zdefiniowanie w nim struktury akcji:

cd /arm_ws/src/fibonacci_sequence_action
mkdir action && cd action
touch Fibonacci.action

Należy zmodyfikować plik Fibonacci.action (np. korzystając z nano, gedit lub VS code):

int32 order
---
int32[] sequence
---
int32[] partial_sequence

gdzie order to cel akcji, sequence to wynik, a partial_sequence to feedback.

Mając zdefiniowaną strukturę akcji możemy ją dodać do pipelineu generowania kodu interfejsu wewnętrznego ROS2 rosidl. W tym celu konieczne jest zmodyfikowanie pliku CMakeLists.txt (przed linijką ament_package()) tak, aby zawierał:

find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "action/Fibonacci.action"
)

Należy również dodać wymagane zależności do pliku package.xml:

<buildtool_depend>rosidl_default_generators</buildtool_depend>

<depend>action_msgs</depend>

<member_of_group>rosidl_interface_packages</member_of_group>

Na tym etapie powinno być możliwe zbudowanie definicji akcji:

cd /arm_ws
source /opt/ros/lyrical/setup.bash
colcon build --symlink-install

Jeżeli budowanie zakończyło się pomyślnie, możliwe jest podejrzenie definicji akcji z poziomu interfejsu ROS2:

source /arm_ws/install/setup.bash
ros2 interface show fibonacci_sequence_action/action/Fibonacci

Przygotowanie skryptu serwera akcji

Wiedząc, że nowa paczka się buduje, a ROS2 udostpęnia strukturę akcji, możemy przystąpić do pisania skryptu serwera akcji.

Należy zacząć od utworzenia folderu scripts i pliku fibonacci_action_server.py:

cd /arm_ws/src/fibonacci_sequence_action/
mkdir scripts && cd scripts
touch fibonacci_action_server.py

Plik fibonacci_action_server.py powinien zostać zmodyfikowany następująco:

#!/usr/bin/env python3
import time

import rclpy
from rclpy.action import ActionServer
from rclpy.node import Node

from fibonacci_sequence_action.action import Fibonacci


class FibonacciActionServer(Node):

    def __init__(self):
        super().__init__('fibonacci_action_server')
        self._action_server = ActionServer(
            self,
            Fibonacci,
            'fibonacci',
            self.execute_callback)
        self.get_logger().info('Fibonacci Action Server is running...')

    def execute_callback(self, goal_handle):
        self.get_logger().info('Executing goal...')

        feedback_msg = Fibonacci.Feedback()
        feedback_msg.partial_sequence = [0, 1]

        for i in range(1, goal_handle.request.order):
            feedback_msg.partial_sequence.append(
                feedback_msg.partial_sequence[i] + feedback_msg.partial_sequence[i-1])
            self.get_logger().info('Feedback: {0}'.format(feedback_msg.partial_sequence))
            goal_handle.publish_feedback(feedback_msg)
            time.sleep(1)

        goal_handle.succeed()

        result = Fibonacci.Result()
        result.sequence = feedback_msg.partial_sequence
        return result


def main(args=None):
    rclpy.init(args=args)

    fibonacci_action_server = FibonacciActionServer()

    rclpy.spin(fibonacci_action_server)


if __name__ == '__main__':
    main()

Należałoby także oznaczyć ten plik jako wykonywalny:

chmod +x fibonacci_action_server.py

Niezbędne będzie również dodanie ścieżki scripts/fibonacci_action_server.py i paczek rclpy oraz ament_cmake_python do pliku CMakeLists.txt:

find_package(ament_cmake_python REQUIRED)
find_package(rclpy REQUIRED)

install(PROGRAMS
  scripts/fibonacci_action_server.py
  DESTINATION lib/${PROJECT_NAME}
)

oraz odpowiednich zależności do pliku package.xml:

  <buildtool_depend>ament_cmake_python</buildtool_depend>

  <depend>rclpy</depend>

Prace nad serwerem akcji zostały zakończone, możliwe jest teraz zbudowanie paczki i uruchomienie:

cd /arm_ws
colcon build --symlink-install
source install/setup.bash
ros2 run fibonacci_sequence_action fibonacci_action_server.py

W osobnym terminalu możemy sprawdzić listę dostępnych akcji:

ros2 action list

oraz wywołać wykonywanie akcji:

ros2 action send_goal --feedback /fibonacci fibonacci_sequence_action/action/Fibonacci "{order: 10}"

W przypadku błędu o braku pliku wykonywalnego należy usunąć foldery build, install i log, oraz ponownie przebudować workspace:

rm -r /arm_ws/build /arm_ws/install /arm_ws/log
source /opt/ros/lyrical/setup.bash
colcon build --symlink-install

5. Zadanie do samodzielnej realizacji

W ramach zadania należy przygotować akcję spełniającą założenia z punktu 3. W tym celu należy wykorzystać symulację:

cd /arm_ws
source install/setup.bash
ros2 launch arm02 arm02_world.launch.py

Udostępnia ona dwa topic’i:

• /laser_scan (sensor_msgs/msg/LaserScan)
• /cmd_vel (geometry_msgs/msg/Twist)

Pierwszy informuje o odległości od przeszkody, drugi natomiast powinien być wykorzystany do ruchu (liniowo, w osi x).

Wsparcie w zakresie pisania subscriberów w ROS2 można znaleźć tutaj.

Akcja powinna być zdefiniowana tak, jak poniżej:

float32 goal_distance
float32 timeout
float32 precision
---
bool succeeded
float32 final_precision
float32 total_time
---
float32 current_distance
float32 estimated_time

UWAGA! Nazwa pliku action musi zaczynać się z wielkiej litery i zawierać wyłącznie litery oraz cyfry.

Na eKursy proszę udostępnić:

• skrypt serwera akcji (proszę zmienić rozszerzenie na .txt)
• zrzut ekranu terminala z widocznym feedbackiem
• zrzuty ekranu terminala z widocznym resultem (w sytuacji kiedy został przekroczony timeout oraz kiedy udało się wykonać akcję)

_{Autor: Kamil Młodzikowski
Na podstawie: Creating an action oraz Writing an action server and client (Python) ze strony docs.ros.org.}